import pandas as pd
import matplotlib.pyplot as plt
import numpy as np


def spherical_to_cartesian(phi, theta):
    # x = 360 * np.sin(np.radians(theta)) * np.cos(np.radians(phi)) + 180
    # y = 360 * np.sin(np.radians(theta)) * np.sin(np.radians(phi)) + 180
    x = theta
    y = phi
    return x, y


def plot_trace_xy(file_path):
    # 读取CSV文件
    df = pd.read_csv(file_path)
    # 初始化列表存储转换后的坐标
    ori_x, ori_y = [], []
    enc_x, enc_y = [], []
    out_x, out_y = [], []
    # 转换每一行的数据
    for index, row in df.iterrows():
        phi_ori, theta_ori = map(float, row['point_ori'].strip('()').split(','))
        phi_enc, theta_enc = map(float, row['point_enc'].strip('()').split(','))
        phi_out, theta_out = map(float, row['point_out'].strip('()').split(','))

        x_ori, y_ori = spherical_to_cartesian(phi_ori, theta_ori)
        x_enc, y_enc = spherical_to_cartesian(phi_enc, theta_enc)
        x_out, y_out = spherical_to_cartesian(phi_out, theta_out)

        ori_x.append(x_ori)
        ori_y.append(y_ori)
        enc_x.append(x_enc)
        enc_y.append(y_enc)
        out_x.append(x_out)
        out_y.append(y_out)

    # 创建图形和轴
    plt.figure(figsize=(10, 6))

    # 绘制折线图
    plt.plot(ori_x, ori_y, label='point_ori')
    plt.plot(enc_x, enc_y, label='point_enc')
    plt.plot(out_x, out_y, label='point_out')

    # 添加标题和标签
    plt.title('Trajectories of point_ori, point_enc, and point_out')
    plt.xlabel('X')
    plt.ylabel('Y')

    # 添加图例
    plt.legend()

    # 显示网格
    plt.grid(True)

    # 显示图形
    plt.show()



def plot_psll(file_path):
    # 读取CSV文件
    df = pd.read_csv(file_path)
    # 创建图形和轴
    plt.figure(figsize=(10, 6))
    # 绘制折线图
    plt.plot(df['psll_ori'], label='psll_ori')
    plt.plot(df['psll_enc'], label='psll_enc')
    plt.plot(df['psll_out'], label='psll_out')
    # 添加标题和标签
    # plt.title('Plot of psll_ori, psll_enc, and psll_out')
    plt.xlabel('Index')
    plt.ylabel('PSLL')
    # 添加图例
    plt.legend()
    # 显示图形
    plt.grid(True)
    plt.show()


def main():
    # 画PSLL图
    # plot_psll("../files/track/[ae2d]lstm/dataset-uav-01/dataset_test/compare/y_out_psll_point.csv")
    # 画轨迹图
    plot_trace_xy("../files/track/[ae2d]lstm/dataset-uav-01-interp-3000/dataset_test/compare/y_out_psll_point.csv")



if __name__=="__main__":
    main()
